% SLAM算法对比工具（激光雷达版）

clear all;
close all;
clc;

fprintf('========================================\n');
fprintf('  SLAM算法对比分析系统\n');
fprintf('========================================\n\n');

%% 轨迹选择
fprintf('轨迹模式选择:\n');
fprintf('1. 固定路径点轨迹（可重复，适合对比）\n');
fprintf('2. 动态避障轨迹（随机探索）\n');
fprintf('----------------------------------------\n');
choice = input('请选择轨迹模式 [1/2] (默认1): ', 's');
if isempty(choice)
    choice = '1';
end

%% 添加路径
script_path = fileparts(mfilename('fullpath'));
addpath(fullfile(script_path, '..', 'common'));
addpath(fullfile(script_path, '..', 'ekf_slam'));
addpath(fullfile(script_path, '..', 'graph_slam'));
addpath(fullfile(script_path, '..', 'fastslam'));
addpath(fullfile(script_path, '..', 'occupancy_grid'));

%% 加载统一配置
config = get_slam_config();

% 应用选择
if choice == '1'
    config.use_fixed_trajectory = true;
    fprintf('\n使用固定路径点轨迹\n');
elseif choice == '2'
    config.use_fixed_trajectory = false;
    fprintf('\n使用动态避障轨迹\n');
else
    fprintf('\n无效选择，使用默认（固定路径点）\n');
    config.use_fixed_trajectory = true;
end
fprintf('\n');

% 解包常用参数
dt = config.dt;
total_time = config.total_time;
steps = config.steps;
Q = config.Q;
R = config.R;
laser_range = config.laser_range;
laser_fov = config.laser_fov;
num_beams = config.num_beams;
laser_noise = config.laser_noise;
USE_FIXED_TRAJECTORY = config.use_fixed_trajectory;
v_base = config.v_base;
omega_base = config.omega_base;

% 统一环境地图
[landmarks, obstacles] = generate_landmarks();
n_obstacles = size(obstacles, 1);

% 生成真实轨迹或准备动态参数
if USE_FIXED_TRAJECTORY
    % 【路径点轨迹模式】预生成轨迹
    initial_state = config.initial_state;
    fprintf('生成真实轨迹（固定路径点轨迹）...\n');
    controls = generate_waypoint_trajectory(config.waypoints, dt, total_time, initial_state);
    
    % 使用固定控制序列仿真
    laser_params = struct('dt', dt, 'range', laser_range, 'fov', laser_fov, ...
                          'num_beams', num_beams, 'noise', laser_noise);
    noise_params = struct('Q', Q);
    [true_trajectory, ~] = simulate_with_fixed_trajectory(controls, obstacles, ...
                                                           laser_params, noise_params, initial_state);
    true_trajectory_generated = true;
else
    % 【动态避障轨迹模式】由第一个算法实时生成
    fprintf('准备动态避障轨迹生成参数...\n');
    fprintf('第一个算法（EKF-SLAM）将实时生成轨迹，后续算法使用相同轨迹\n');
    true_trajectory = [];  % 暂时为空，由EKF-SLAM生成
    true_trajectory_generated = false;
end

%% 运行EKF-SLAM（动态模式下同时生成真实轨迹）
fprintf('\n========================================\n');
fprintf('【1/4】运行EKF-SLAM + 激光雷达...\n');
fprintf('========================================\n');
tic;

if ~true_trajectory_generated
    % 动态模式：EKF-SLAM实时生成轨迹，后续算法共享此轨迹
    fprintf('  动态模式：实时生成轨迹...\n');
    [ekf_trajectory, ekf_landmarks, ekf_stats, true_trajectory] = run_ekf_slam([], obstacles, ...
                                                 dt, Q, R, laser_range, laser_fov, num_beams, laser_noise, true, ...
                                                 v_base, omega_base);
    fprintf('  真实轨迹已生成（随机初始方向: %.2f rad）\n', true_trajectory(3, 1));
    fprintf('  后续算法将使用相同轨迹以确保公平对比\n');
else
    % 固定模式：使用预生成的轨迹
    [ekf_trajectory, ekf_landmarks, ekf_stats] = run_ekf_slam(true_trajectory, obstacles, ...
                                                 dt, Q, R, laser_range, laser_fov, num_beams, laser_noise, true);
end

ekf_time = toc;
ekf_stats.runtime = ekf_time;
fprintf('  完成. 耗时: %.2fs, 路标数: %d\n', ekf_time, ekf_landmarks.map.Count);

% 绘制EKF-SLAM结果
plot_single_result('EKF-SLAM', true_trajectory, ekf_trajectory, ekf_landmarks, obstacles, ekf_stats);

%% 运行Graph-SLAM
fprintf('\n========================================\n');
fprintf('【2/4】运行Graph-SLAM + 激光雷达...\n');
fprintf('========================================\n');
tic;
[graph_trajectory, graph_landmarks, graph_stats] = run_graph_slam(true_trajectory, obstacles, ...
                                                   dt, Q, R, laser_range, laser_fov, num_beams, laser_noise, true);
graph_time = toc;
graph_stats.runtime = graph_time;  % 添加运行时间
fprintf('  完成. 耗时: %.2fs, 特征数: %d\n', graph_time, graph_landmarks.Count);

% 绘制Graph-SLAM结果
plot_single_result('Graph-SLAM', true_trajectory, graph_trajectory, graph_landmarks, obstacles, graph_stats);

%% 运行FastSLAM
fprintf('\n========================================\n');
fprintf('【3/4】运行FastSLAM + 激光雷达...\n');
fprintf('========================================\n');
tic;
params = struct('dt', dt, 'Q', Q, 'R', R, ...
                'laser_range', laser_range, 'laser_fov', laser_fov, ...
                'num_beams', num_beams, 'laser_noise', laser_noise, ...
                'show_progress', true);
fastslam_result = run_fastslam(true_trajectory, obstacles, params);
fastslam_trajectory = fastslam_result.trajectory;
fastslam_time = toc;

% 计算FastSLAM统计
fastslam_errors = sqrt(sum((true_trajectory(1:2, :) - fastslam_trajectory(1:2, :)).^2, 1));
fastslam_stats = struct();
fastslam_stats.errors = fastslam_errors;
fastslam_stats.avg_error = mean(fastslam_errors);
fastslam_stats.max_error = max(fastslam_errors);
fastslam_stats.n_landmarks = length(fastslam_result.discovered_landmarks);
fastslam_stats.runtime = fastslam_time;  % 添加运行时间
fprintf('  完成. 耗时: %.2fs, 特征数: %d\n', fastslam_time, fastslam_stats.n_landmarks);

% 绘制FastSLAM结果
plot_single_result('FastSLAM', true_trajectory, fastslam_trajectory, fastslam_result, obstacles, fastslam_stats);

%% 运行Occupancy Grid SLAM
fprintf('\n========================================\n');
fprintf('【4/4】运行Occupancy Grid SLAM + MCL...\n');
fprintf('========================================\n');
tic;
[grid_trajectory, grid_map, grid_stats] = run_grid_slam(true_trajectory, obstacles, ...
                                          dt, Q, laser_range, laser_fov, num_beams, laser_noise, true);
grid_time = toc;
grid_stats.runtime = grid_time;  % 添加运行时间
fprintf('  完成. 耗时: %.2fs, 地图覆盖率: %.1f%%\n', grid_time, grid_stats.map_coverage);

% 绘制Grid-SLAM结果
plot_grid_result('Grid SLAM + MCL', true_trajectory, grid_trajectory, grid_map, obstacles, grid_stats);

%% 对比分析
fprintf('\n========================================\n');
fprintf('  对比结果\n');
fprintf('========================================\n\n');

fprintf('【路标SLAM对比】\n');
fprintf('算法          平均误差   最大误差   路标数   耗时\n');
fprintf('----------------------------------------------------\n');
fprintf('EKF-SLAM     %.3fm     %.3fm    %4d    %.2fs\n', ...
        ekf_stats.avg_error, ekf_stats.max_error, ekf_stats.n_landmarks, ekf_time);
fprintf('Graph-SLAM   %.3fm     %.3fm    %4d    %.2fs\n', ...
        graph_stats.avg_error, graph_stats.max_error, graph_stats.n_landmarks, graph_time);
fprintf('FastSLAM     %.3fm     %.3fm    %4d    %.2fs\n', ...
        fastslam_stats.avg_error, fastslam_stats.max_error, fastslam_stats.n_landmarks, fastslam_time);

fprintf('\n【栅格SLAM】\n');
fprintf('算法          平均误差   最大误差   覆盖率   耗时\n');
fprintf('----------------------------------------------------\n');
fprintf('Grid+MCL     %.3fm     %.3fm    %.1f%%   %.2fs\n', ...
        grid_stats.avg_error, grid_stats.max_error, grid_stats.map_coverage, grid_time);

fprintf('\n【运行时间对比】\n');
fprintf('算法          运行时间   相对速度\n');
fprintf('----------------------------------------------------\n');
min_time = min([ekf_time, graph_time, fastslam_time, grid_time]);
fprintf('EKF-SLAM     %.2fs      %.2fx\n', ekf_time, ekf_time/min_time);
fprintf('Graph-SLAM   %.2fs      %.2fx\n', graph_time, graph_time/min_time);
fprintf('FastSLAM     %.2fs      %.2fx\n', fastslam_time, fastslam_time/min_time);
fprintf('Grid+MCL     %.2fs      %.2fx\n', grid_time, grid_time/min_time);
fprintf('总耗时: %.2fs\n', ekf_time + graph_time + fastslam_time + grid_time);

%% 可视化对比
plot_comparison(true_trajectory, ekf_trajectory, graph_trajectory, fastslam_trajectory, grid_trajectory, ...
                landmarks, ekf_landmarks, graph_landmarks, fastslam_result, grid_map, ...
                ekf_stats, graph_stats, fastslam_stats, grid_stats);

% 保存结果
save('comparison_results.mat', 'true_trajectory', 'ekf_trajectory', 'graph_trajectory', ...
     'fastslam_trajectory', 'grid_trajectory', 'ekf_landmarks', 'graph_landmarks', ...
     'fastslam_result', 'grid_map', 'ekf_stats', 'graph_stats', 'fastslam_stats', 'grid_stats');
fprintf('\n结果已保存到 comparison_results.mat\n');

fprintf('\n========================================\n');
fprintf('  算法对比完成！\n');
fprintf('  结果已保存到 comparison_results.mat\n');
fprintf('========================================\n');

%% 辅助函数：绘制单个算法结果（特征点SLAM）
function plot_single_result(alg_name, true_traj, est_traj, landmarks_data, obstacles, stats)
    figure('Name', sprintf('%s 运行结果', alg_name), 'Position', [100, 100, 1000, 800]);
    
    % 绘制障碍物
    hold on;
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                  'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 1.5);
    end
    
    % 绘制真实轨迹
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
    
    % 绘制估计轨迹
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 2, 'DisplayName', '估计轨迹');
    
    % 绘制起点和终点
    plot(true_traj(1, 1), true_traj(2, 1), 'go', 'MarkerSize', 12, ...
         'MarkerFaceColor', 'g', 'DisplayName', '起点');
    plot(true_traj(1, end), true_traj(2, end), 'rs', 'MarkerSize', 12, ...
         'MarkerFaceColor', 'r', 'DisplayName', '终点');
    
    % 绘制路标
    if isstruct(landmarks_data)
        % EKF-SLAM 和 Graph-SLAM
        if isfield(landmarks_data, 'positions') && ~isempty(landmarks_data.positions)
            plot(landmarks_data.positions(1, :), landmarks_data.positions(2, :), ...
                 'r*', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '估计路标');
        end
    elseif iscell(landmarks_data)
        % FastSLAM
        if ~isempty(landmarks_data)
            lm_pos = cell2mat(landmarks_data);
            if ~isempty(lm_pos)
                plot(lm_pos(1, :), lm_pos(2, :), 'r*', 'MarkerSize', 10, ...
                     'LineWidth', 2, 'DisplayName', '估计路标');
            end
        end
    end
    
    % 设置图形属性
    axis equal;
    xlim([0, 50]);
    ylim([0, 50]);
    xlabel('X (m)', 'FontSize', 12, 'FontWeight', 'bold');
    ylabel('Y (m)', 'FontSize', 12, 'FontWeight', 'bold');
    
    % 标题包含性能信息
    title_str = sprintf('%s - 平均误差: %.3fm | 最大误差: %.3fm | 路标数: %d | 耗时: %.2fs', ...
                        alg_name, stats.avg_error, stats.max_error, stats.n_landmarks, stats.runtime);
    title(title_str, 'FontSize', 14, 'FontWeight', 'bold');
    
    legend('Location', 'northeast', 'FontSize', 10);
    grid on;
    hold off;
    
    drawnow;
end

%% 辅助函数：绘制Grid-SLAM结果
function plot_grid_result(alg_name, true_traj, est_traj, grid_map, obstacles, stats)
    figure('Name', sprintf('%s 运行结果', alg_name), 'Position', [100, 100, 1200, 500]);
    
    % 提取占据栅格数据
    if isstruct(grid_map)
        occ_map = grid_map.occupancy;
        grid_resolution = grid_map.resolution;
        map_size = grid_map.size;
    else
        occ_map = grid_map;
        grid_resolution = 0.2;
        map_size = 50;  % 默认地图尺寸
    end
    
    % 转换log-odds到概率（与main_grid_slam.m相同的方法）
    prob_map_display = 1 - 1./(1 + exp(occ_map));
    
    % 左图：占据栅格地图 + 轨迹（使用真实世界坐标）
    subplot(1, 2, 1);
    hold on; axis equal;
    
    % 显示占据栅格地图（直接使用真实世界坐标）
    imagesc([0, map_size], [0, map_size], prob_map_display);
    colormap(gca, flipud(gray));
    caxis([0, 1]);
    axis xy;  % 设置坐标轴方向：x向右，y向上
    
    % 添加颜色条
    cbar = colorbar;
    ylabel(cbar, '占据概率', 'FontSize', 10);
    
    % 绘制真实轨迹（直接使用真实坐标，无需转换）
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
    
    % 绘制估计轨迹（直接使用真实坐标，无需转换）
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 2, 'DisplayName', '估计轨迹');
    
    % 标记起点和终点
    plot(true_traj(1, 1), true_traj(2, 1), 'ro', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '起点');
    plot(true_traj(1, end), true_traj(2, end), 'rs', 'MarkerSize', 10, 'LineWidth', 2, 'DisplayName', '终点');
    
    xlabel('X (m)', 'FontSize', 11, 'FontWeight', 'bold');
    ylabel('Y (m)', 'FontSize', 11, 'FontWeight', 'bold');
    title(sprintf('占据栅格地图 (覆盖率: %.1f%%)', stats.map_coverage), 'FontSize', 12, 'FontWeight', 'bold');
    legend('Location', 'best', 'FontSize', 9);
    xlim([0, map_size]); ylim([0, map_size]);
    grid on;
    hold off;
    
    % 右图：环境地图 + 轨迹
    subplot(1, 2, 2);
    hold on;
    
    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                  'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 1.5);
    end
    
    % 绘制轨迹
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'DisplayName', '真实轨迹');
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 2, 'DisplayName', '估计轨迹');
    plot(true_traj(1, 1), true_traj(2, 1), 'go', 'MarkerSize', 12, ...
         'MarkerFaceColor', 'g', 'DisplayName', '起点');
    plot(true_traj(1, end), true_traj(2, end), 'rs', 'MarkerSize', 12, ...
         'MarkerFaceColor', 'r', 'DisplayName', '终点');
    
    axis equal;
    xlim([0, 50]);
    ylim([0, 50]);
    xlabel('X (m)', 'FontSize', 11, 'FontWeight', 'bold');
    ylabel('Y (m)', 'FontSize', 11, 'FontWeight', 'bold');
    
    title_str = sprintf('%s - 平均误差: %.3fm | 最大误差: %.3fm | 耗时: %.2fs', ...
                        alg_name, stats.avg_error, stats.max_error, stats.runtime);
    title(title_str, 'FontSize', 12, 'FontWeight', 'bold');
    legend('Location', 'northeast', 'FontSize', 9);
    grid on;
    hold off;
    
    drawnow;
end
